#ifndef RTDECONTROL_H
#define RTDECONTROL_H
#include <memory>
#include <vector>
#include <functional>
#include "RTDEProtocol.h"

namespace CURCONTROL {
class RTDEClient;
class RobotState;
}

namespace CURCONTROL {

/*!
* \brief rtde控制类：关节运动、轨迹运动、运动学、获取机械臂实时数据
*/
class RTDEControl
{ 
private:
    std::shared_ptr<RTDEClient> rtde_;// rtde句柄
    std::shared_ptr<RobotState> robotState_;// 数据

    std::function<void(void*)> statusChangedFunc_;// 循环调用回调函数
public:
    enum ToolVoltage{
        TOOL_VOLTAGE_0_V = 0,
        TOOL_VOLTAGE_12_V = 12,
        TOOL_VOLTAGE_24_V = 24
    };
public:
    RTDEControl();
    ~RTDEControl();
    /*!
   * \brief 启动RTDE流程
   */
    void StartRTDE();
    /*!
   * \brief 设置机械臂tool电压
   */
    void SetToolVoltage(ToolVoltage voltage);
    /*!
   * \brief 停止rtde脚本中的servo线程
   */
    void StopServoThread();
    /*!
   * \brief 使能机械臂freedrive
   */
    void FreedriveMode();
    /*!
   * \brief 关闭机械臂freedrive
   */
    void EndFreedriveMode();
    /*!
   * \brief 设置标准数字输出状态
   *
   * \param[in] 数字输出端口号
   * \param[in] 要设置的状态,true为有效
   */
    void SetStdDigitalOut(uint8_t id, bool status);
    /*!
   * \brief 获取机械臂的控制频率
   */
    uint32_t GetFrequency() const;
    /*!
   * \brief 设置循环调用回调函数
   */
    void SetStatusChangedCallback(std::function<void(void*)> statusChangedFunc);
    /*!
   * \brief 查询RTDE是否启动状态
   *
   * \returns 已启动返回true
   */
    bool IsStarted() const;
    /*!
   * \brief 是否链接
   *
   * \returns 链接成功返回true
   */
    bool IsConnected() const;
    /*!
   * \brief 与机械臂建立RTDE链接
   *
   * \param[in] 机械臂ip
   *
   * \returns 链接成功返回true
   */
    bool Connect(const std::string& robotIp);
    /*!
   * \brief 初始化
   *
   */
    void Init();
    /*!
   * \brief 与客户端重新链接
   *
   * \returns 链接成功返回true
   */
    bool Reconnect();
    /*!
   * \brief 与客户端断开链接
   */
    void Disconnect();
    /**
       * @brief Servo to position (linear in joint-space)
       * @param q joint positions [rad]
       * @param speed NOT used in current version
       * @param acceleration NOT used in current version
       * @param time time where the command is controlling the robot. The function is blocking for time t [S]
       * @param lookahead_time time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
       * @param gain proportional gain for following target position, range [100,2000]
       */
    bool ServoJ(const std::vector<double> &q, double speed, double acceleration, double time,
                double lookahead_time, double gain);
    /**
       * @brief Servo to position (linear in tool-space)
       * @param pose target pose
       * @param speed NOT used in current version
       * @param acceleration NOT used in current version
       * @param time time where the command is controlling the robot. The function is blocking for time t [S]
       * @param lookahead_time time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
       * @param gain proportional gain for following target position, range [100,2000]
       */
    bool ServoJ_POS(const std::vector<double> &pose, double speed, double acceleration, double time,
                double lookahead_time, double gain);
    /*!
    * \brief 关节停止命令
    *
    * \param[in] 停下的加速度
    */
    void StopJ(double a = 2.0);
    /**
      * @brief This function will terminate the script on controller.
      */
    void StopScript();
    /*!
    * \brief UR 脚本是否在运行状态
    */
    bool IsProgramRunning() const;
    /*!
    * \brief 查询UR 脚本的状态
    */
    RuntimeState GetProgramState() const;
    /*!
    * \brief UR 是否在保护性停止状态
    */
    bool IsProtectiveStopped() const;
    /*!
    * \brief UR 是否在紧急停止状态
    */
    bool IsEmergencyStopped() const;
    /*!
    * \brief 获取UR当前TCP的位姿信息
    */
    std::vector<double> GetActualTcpPose() const;
    /*!
    * \brief 获取UR当前关节角度
    */
    std::vector<double> GetActualJoint() const;
    /*!
    * \brief 获取UR当前tcp力距
    */
    std::vector<double> GetActualTcpForce() const;
    /*!
    * \brief 获取UR当前关节电流
    */
    std::vector<double> GetActualCurrent() const;
    /*!
    * \brief 获取UR当前关节温度
    */
    std::vector<double> GetActualTemperatures() const;
    /*!
    * \brief 获取UR各个关节的模式
    */
    std::vector<int32_t> GetJointMode() const;
    /*!
    * \brief 获取UR robot mode
    */
    RobotMode GetRobotMode() const;
    /*!
    * \brief 获取UR robot status
    */
    RobotStatus GetRobotStatus() const;
    /*!
    * \brief 获取UR safety mode
    */
    SafetyMode GetSafetyMode() const;
    /*!
    * \brief 获取UR 数字输入状态
    */
    uint64_t GetDigitalInputBits() const;
    /*!
    * \brief 获取UR 数字输出状态
    */
    uint64_t GetDigitalOutputBits() const;
    /*!
   * \brief 修改控制使能标志
   *
   * \param[in] 是否使能
   */
    void SetSendEnable(bool enable);
    /*!
   * \brief 以下接口均为直接操作寄存器，无视rtde控制脚本
   * \brief 接口作用的条件：RTDE流程已启动
   */
    /*!
   * \brief 清除命令寄存器的值
   */
    void SendClearCommand(std::string debug);
    /*!
   * \brief 发送当前角度给寄存器
   */
    void SendClearServojReg();

private:
    /*!
   * \brief 解析RTDE端口的数据
   *
   * \param[in] 获取的待解析的通信数据
   */
    void ReceiveCallback(std::vector<uint8_t> &bytes);
    /*!
   * \brief 发送控制指令
   *
   * \returns 控制指令执行成功返回true
   */
    bool SendCommand(const RTDEProtocol &command);
    /*!
   * \brief 获取控制脚本命令执行状态值
   *
   * \returns 返回脚本命令执行状态值
   */
    ControlScriptState GetControlScriptState();
    /*!
   * \brief 获取某个寄存器值
   *
   * \param[in] 寄存器编号
   *
   * \returns 返回寄存器值
   */
    int GetOutIntReg(int reg);
};

}// namespace CURCONTROL

#endif // RTDECONTROL_H
